from ros2webserver.baseserver.capability import Capability
from geometry_msgs.msg import Twist
import json

class NavCmdVel(Capability):

    def __init__(self, protocol):
        # Call superclass constructor
        Capability.__init__(self, protocol)
        # Register the operations that this capability provides
        protocol.register_operation("moveController", self.move_control)

        # Create a publisher for nav_cmd_vel topic
        self._cmd_vel_pub = self.protocol.node_handle.create_publisher(
            Twist,
            "cmd_vel_nav",
            10,
        )
        

    def move_control(self, message):
        self.protocol.node_handle.get_logger().info(f"{message}")

        twist_msg = Twist()
        twist_msg.linear.x = message['data']['linearSpeed']
        twist_msg.angular.z = message['data']['angleSpeed']
        self._cmd_vel_pub.publish(twist_msg)

        self.protocol.send({
            "op":"moveController",
            "code":1,
            "msg":None,  
            "data":None
        })

    def finish(self):
        self.protocol.node_handle.destroy_publisher(self._cmd_vel_pub)
